#include <cubEMath.hh>
#include <cubEPrimitive.hh>
#include <math.h>

namespace CubE
{

//-------------global variables and functions-----------
   bool IsFloatEqual(float a, float b)
   {
      return (fabs(a-b)<0.000001f) ? true : false;
   }
   
//-------------Degree-----------
   Degree::Degree(void)
      :_value(0.0f)
   {
      
   }

   Degree::Degree(float val)
      :_value(val)
   {

   }

   Degree::Degree(Radian rad)
   {
      _value = (rad.Value() * 180) / PI ;
   }

//-------------Radian-----------
   Radian::Radian(void)
      :_value(0.0f)
   {
      
   }

   Radian::Radian(float val)
      :_value(val)
   {
	  
   }

   Radian::Radian(Degree deg)
   {
      _value = (deg.Value() * PI) / 180 ;
   }
   

//-------------Quaternion-----------
   
   /*About any algorithm regarding to Quternion it comes from
    * paper Q u a t e r n i o n s by Ken Shoemake  */

   Quaternion::Quaternion(void)
   {
      _iVec = Vector3::ZERO;
      _rW = 0.0f;
   }

   Quaternion::Quaternion(Quaternion const& other)
      :_iVec(other._iVec),
       _rW(other._rW)
   {
      
   }

   Quaternion::Quaternion(Vector3 vec, Radian angle)
   {
      //1. First to normalize the vec to form the unit-axis
//      Vector3 vec = axis;
      vec.Normalize();
      
      //2. Generate the Quaternion according to the algorithm
      initialize( vec*sin(angle.Value()/2), cos(angle.Value()/2) );
   }

   Quaternion::Quaternion(Vector3 vec, Degree angle)
   {
      //1. First to normalize the vec to form the unit-axis
      vec.Normalize();

      //2. Convert Degree to Radian
      Radian rd = Radian(angle);

      //3. Generate the Quaternion according to the algorithm
      initialize( vec*sin(rd.Value()/2), cos(rd.Value()/2) );
   }

   Quaternion::Quaternion(Vector3 iVec, float rW)
   {
      _iVec = iVec ;
      _rW = rW ;
   }
   
   Quaternion::Quaternion(const Vector3& fromV, const Vector3& destV)
   {
      //1. First to normalize the two vectors
      Vector3 from(fromV);
      Vector3 dest(destV);
      if(!from.IsNormalized()) {
	 from.Normalize();
      }
      if(!dest.IsNormalized()) {
	 dest.Normalize();
      }

      //2. Then get their outerproduct vector that is
      //   perpendicular both to the two src vectors
      Vector3 axis = from.CrossProduct(dest);

      //3. And get its length and normalize it to be a unit
      //   vector
      float axis_length = axis.Normalize();

      //4. Since we have such a formula:
      
           /*   if: c = a x b
            *   then: |c| = |a|*|b|*sin<a,b> */
      
      //   And we also know that the two src vectors'
      //   length both equals to 1(they've been normalized).
      //   So the length of the outerproduct equals to
      //   sin<a,b>. And we can get the value of the angle
      //   by using the arcsin() function.
      
      float sita = asin(axis_length); // calling math lib

      //5. But we need to have a sense that, the angle we
      //   got is the one of the two possible candidates.
      //   That's because the result of function arcsin()
      //   is limited between [-PI/2, PI/2], but actually
      //   we always measure the angle between two vectors
      //   between [-2*PI, 2*PI](negative angle means a
      //   reverse result vector). So it exists a potential
      //   candidate, say, maybe actuall 150degree if we
      //   get a result of 30degree.
      //
      //   The Dot Product(innerproduct) is a good way to
      //   help us choose the right one. Since we have such
      //   formula below:
      
           // d = a * b = |a|*|b|*cos<a,b>

      //   Since we already knew that both a and b are unit
      //   vectors, the result of dot product equals to the
      //   function cos() for that angle, which is exactly 
      //   the one we discussed above. So we can kickoff one
      //   invalided value through the polarity of the cos().
      //    
      //   If cos() is negative, we then need PI to minus the
      //   angle and reassign to it.

      //   For example, we got a result -60degree between
      //   two vectors, and We know that maybe it's actually
      //   -120degree. Then we dot producct them and find the
      //   innerproduct < 0. Since cos(-150degree)<0 So we hence
      //   kickoff the -60degree and remain -150degree, which is
      //   exactly what we want.

      float dot_product = from.DotProduct(dest);
      if( dot_product<0) {
	 sita = PI - sita ;
      }

      //6.Because there is a theorem for Quaternion which says,
          /*If N(q)  = 1, then q = [vö  sin W, cos W]
	    acts to rotate around unit axis vö  by 2W. */

      // Since we knew the sita is the angle that the "from"
      // vector rotates to "dest" vector. So obviously the
      // Quaternion which its w equals to sita/2 can make
      // "from" vector rotate to "dest" vector by sita.
      
      sita = sita/2;

      //7.Finally initialize this Quaternion.
      initialize(axis*sin(sita), cos(sita));
   }

   void Quaternion::initialize(Vector3 imaginary_part, float real_part)
   {
      _iVec = imaginary_part;
      _rW = real_part;
   }

   void Quaternion::initialize(float x, float y, float z, float w)
   {
      _iVec.Set(x, y, z);
      _rW = w;
   }

   Quaternion Quaternion::GetConjugate(void) const
   {
      return Quaternion(_iVec*(-1), _rW);
   }

   Quaternion Quaternion::operator*(const Quaternion& other) const
   {
      Quaternion ret;
      Vector3 cross =_iVec.CrossProduct(other._iVec);
      ret._iVec = cross + (other._iVec*_rW) + (_iVec*other._rW);
      ret._rW = _rW * other._rW - _iVec.DotProduct(other._iVec);
      return ret;
   }

   void Quaternion::Normalize(void)
   {
      float length = GetLength();
      if(IsFloatEqual(length, 0.0f)) {
	 return ;
      }

      _iVec.Set(_iVec[0]/length,
		_iVec[1]/length,
		_iVec[2]/length);
      
      _rW = _rW/length;
   }

   bool Quaternion::IsNormalized(void)
   {
      float length = GetLength();
      return IsFloatEqual(length, 1.0f);
   }

   float Quaternion::GetLength(void)
   {
      float length = sqrt( _iVec[0]* _iVec[0] +
			   _iVec[1]* _iVec[1] +
			   _iVec[2]* _iVec[2] +
			   _rW     * _rW     );

      return length;

   }
}
